#include "StdAfx.h"
#include "CoSyNE.h"

//-----------------------------------------------------------------------------
CoSyNE::CoSyNE(int w_, int c_, int pop_)
:	w				(w_),
	pop			(pop_),
	fitness		(pop_),
	ranking		(pop_),
	permuteIndex(pop_),
	mutateFactor(w_, 1, 1.0f)
{
	std::cout << "CoSyNE(weights = "<<w_<<", cache = "<<c_<<", pop = "<<pop_<<")" << std::endl;

// first dimension (#rows) must be a multiple of 4
	c_ += StrategyWeights;
	w_ = (w_+3) & ~3;
	c_ = (c_+3) & ~3;

	P.resize		(w_, pop_);
	cache.resize(c_, pop_);

	if(P.m()     != w_ || P.n()     != pop_
	|| cache.m() != c_ || cache.n() != pop_)
		std::cout << " construction FAILED!!!";

	std::cout << std::endl;
}

//-----------------------------------------------------------------------------
CoSyNE::~CoSyNE()
{
}


//-----------------------------------------------------------------------------
void CoSyNE::strategyWeightAverage(StrategyWeight i, float x) 
{
	for(int j = 0; j < pop; j++)
		cache(i,j) = x*(0.5f+rng.randf());
}

//-----------------------------------------------------------------------------
float CoSyNE::strategyWeightAverage(StrategyWeight i) const
{
	float sum = 0.0;
	for(int j = 0; j < pop; j++)
		sum += cache(i,j);

	return sum / float(pop);
}


//-----------------------------------------------------------------------------
void CoSyNE::randomizePop()
{
	for(int j = 0; j < pop; j++)
		randomizeSample(j);
}

//-----------------------------------------------------------------------------
//	Higher standard deviation -> higher mutation probability
void CoSyNE::updateMutationFactors(int lambda)
{
	__m128	*stdDev	= (__m128*) &mutateFactor[0],
				f1			= _mm_set1_ps(1.0f/float(lambda)),
				f2			= _mm_set1_ps(1.0f/float(lambda-1));
	M128		sum;
				sum.m128	= _mm_setzero_ps();

//----
//	Get the standard deviation of each weight
//	Do 4 weights at once, with SSE
	for(int n = 0; n < w; n+=4)
	{
		__m128 ss = _mm_setzero_ps(),				// sum of squares
				 s  = _mm_setzero_ps();				// sum

		for(int j = 0; j < lambda; j++)
		{
			__m128 &weights = *(__m128*)&P(n,ranking[j]);

			s	+=     weights;
			ss	+= sqr(weights);
		}

		s *= s;											// square of the sum
		s *= f1;											// average square
		ss -= s;
		ss *= f2;
		s = _mm_sqrt_ps(ss);							// std.dev.
//		s &= _mm_cmpord_ps(s,s);

	//----
		*stdDev++ = s;
		sum.m128 +=(s & _mm_cmpord_ps(s,s));	//	sum, except NaNs
	}

//----
//	Normalize, so the average std.dev. gives a factor of 1
	f1 = _mm_set1_ps(float(w)/(sum.f32[0]+sum.f32[1]+sum.f32[2]+sum.f32[3]));

	for(int n = 0; n < w; n+=4)
	{
		stdDev--;
		__m128 mask = _mm_cmpord_ps(*stdDev,*stdDev);
		*stdDev *= f1;
		*stdDev &= mask;
//		*stdDev |= _mm_andnot_ps(_ps_1,mask);
	}

}//CoSyNE::updateMutationFactors


//-----------------------------------------------------------------------------
void CoSyNE::randomizeSample(int j)
{
//----
//	initialize strategy parameters with the static generator variables
	for(int i = 0; i < StrategyWeights/4; i++)
		((__m128*)cache.getCol(j))[i] = ((__m128*)generatorVariables)[i];

//----
//	mutation of strategy parameters
	sampleMutateStrategyWeights(j,true);

//----
//	random initialize weights
	__m128 gamma = _mm_set1_ps(cache(InitGamma,j)),
			 alpha = _mm_set1_ps(cache(InitAlpha,j)),
			 levy,
			*weights = (__m128*)&P(0,j);

	for(int i = 0; i < w/4; i++)
	{
	//	levy random numbers are quite expensive, so make 4 at once
		levy = rng.levy(gamma,alpha);
		weights[i] = levy;
	}

	sampleClearCache(j);

}//CoSyNE::randomizeSample

//-----------------------------------------------------------------------------
//	Unbiased mutation of that samples strategy parameters
void CoSyNE::sampleMutateStrategyWeights(int j, bool inclInit)
{
	float mutProbMin = std::numeric_limits<float>::epsilon();//1.0f / w;
	float *w = &strategyWeights(j);

//----
//	unbiased mutation of probabilities:
/*	w[MutateProb]	= rng.levyMutation(w[MutateProb], mutProbMin);
	w[RecombProb]	= rng.levyMutation(w[RecombProb]);
*/
	w[MutateProb]	*= (0.5f + rng.randf());
	w[RecombProb]	*= (0.5f + rng.randf());
	if(w[RecombProb] > 1.0f)
		w[RecombProb] = 1.0f;
	if(w[RecombProb] < mutProbMin)
		w[RecombProb] = mutProbMin;
	if(w[MutateProb] > 1.0f)
		w[MutateProb] = 1.0f;
	if(w[MutateProb] < mutProbMin)
		w[MutateProb] = mutProbMin;

//----
//	unbiased mutation of positive real numbers: x *= [1,2) or [0.5,1)
/*	w[MutateAlpha]	= rng.levyMutation(w[MutateAlpha],0.1f,2.0f);
	w[MutateGamma]	= rng.levyMutation(w[MutateGamma],0.0f,0.1f,0.001f);
*/	w[MutateGamma]	*= (0.5f + rng.randf());
	w[MutateAlpha]	*= (0.5f + rng.randf());
	if(w[MutateAlpha] > 2.0f)
		w[MutateAlpha] = 2.0f;
	if(w[MutateAlpha] < 0.1f)
		w[MutateAlpha] = 0.1f;

	if(inclInit)
	{
		w[InitAlpha]	*= (0.5f + rng.randf());
		w[InitGamma]	*= (0.5f + rng.randf());
		w[InitAlpha]	= min(max(w[InitAlpha], 0.1f), 2.0f);
	}

}//CoSyNE::sampleMutateStrategyWeights


//-----------------------------------------------------------------------------
//	Unbiased mutation of the weights of sample j
void CoSyNE::sampleMutateWeights(int j)
{
//----
//	possible mutation of each weight
	double	prob  = 1.0 + cache(MutateProb,j);
	__m128	gamma = _mm_set1_ps(cache(MutateGamma,j)),
				alpha = _mm_set1_ps(cache(MutateAlpha,j));

	M128		levys;
	int		levysLeft = 0;

	for(int i = 0; i < w; i++)
	{
		if(levysLeft==0)
		{
			levys.m128 = rng.levy(gamma,alpha);
			levysLeft = 4;
		}

		if(rng.rand12() < prob)
			P(i,j) += levys.f32[--levysLeft];
	}

}//CoSyNE::mutateWeights

//-----------------------------------------------------------------------------
void CoSyNE::sampleMutateWeightsWeighted(int j)
{
//----
//	possible mutation of each weight
	double	prob  = cache(MutateProb,j),
				gamma = cache(MutateGamma,j),
				alpha = cache(MutateAlpha,j);

	for(int i = 0; i < w; i++)
	{
		if(rng.rand() < prob)
			P(i,j) += (float)rng.levy((0.1+mutateFactor[i])*gamma,alpha);
	}

}//CoSyNE::sampleMutateWeightsWithMuatationFactors

//-----------------------------------------------------------------------------
void CoSyNE::samplesRecombine(int src1, int src2, int dst1, int dst2)
{
	double prob = 1.0 + 0.5 * double(cache(RecombProb,src1) + cache(RecombProb,src2));

	float	*s1 = &P(0,src1),
			*s2 = &P(0,src2),
			*d1 = &P(0,dst1),
			*d2 = &P(0,dst2);

	for(int i = 0; i < w; i++)
	{
		if(rng.rand12() < prob)
			std::swap(d1,d2);

		d1[i] = s1[i];
		d2[i] = s2[i];
	}

//----
//	intermediate recombination (averaging) of strategy parameters
	s1 = &cache(0,src1),
	s2 = &cache(0,src2),
	d1 = &cache(0,dst1),
	d2 = &cache(0,dst2);

	for(int i = 0, lim = cache.m(); i < lim; i++)
	{
		d1[i] =
		d2[i] = 0.5f*(s1[i]+s2[i]);
	}

/*	int	i1 = rng.randInt(w),
			i2 = rng.randInt(w);

	if(i1>i2)
		swp(i1,i2);


	if(rng.rand12() < prob)
		swp(dst1,dst2);

	memcpy(&P(0,dst1), &P(0,src1), i1*sizeof(float));
	memcpy(&P(0,dst2), &P(0,src2), i1*sizeof(float));

	if(rng.rand12() < prob)
		swp(dst1,dst2);

	memcpy(&P(i1,dst1), &P(i1,src1), (i2-i1)*sizeof(float));
	memcpy(&P(i1,dst2), &P(i1,src2), (i2-i1)*sizeof(float));

	if(rng.rand12() < prob)
		swp(dst1,dst2);

	memcpy(&P(i2,dst1), &P(i2,src1), (w-i2)*sizeof(float));
	memcpy(&P(i2,dst2), &P(i2,src2), (w-i2)*sizeof(float));
*/
}//CoSyNE::samplesRecombine



//-----------------------------------------------------------------------------
//	Probabilistic permutation for coevolution
void CoSyNE::samplesPermute(int lambda, double permuteProb)
{
	double factor = permuteProb / pop;

	for(int i = 0; i < w; i++)
	{
		int n = 0;
		for(int j = 1; j < pop-lambda; j++)
		{
		//----
		//	Mark weights for permutation. Likelyhood is inversely proportional to fitness.
			if(rng.rand() < j*factor)
				permuteIndex[n++] = ranking[j];
		}

		if(n>1)
		{
			rng.shuffle<int>(permuteIndex,n);
//			random_shuffle(&permuteIndex[0],&permuteIndex[n]);

			float tmp						= P(i,permuteIndex[0]);
			for(int k = 0; k < n-1; k++)
			{
				P(i,permuteIndex[k])		= P(i,permuteIndex[k+1]);
			}
				P(i,permuteIndex[n-1])	= tmp;

		}//shuffle instances of weight i between selected individuals

	}//for all weights

}//CoSyNE::samplesPermute


//-----------------------------------------------------------------------------
//	Probabilistic permutation for coevolution, slightly optimized
void CoSyNE::samplesPermute(int lambda, double permuteProb, int startWeight, int stopWeight)
{
	double probStep = permuteProb / pop;

	int	*perm = permuteIndex,
			*rank = &ranking[0];
	const int m = P.m();

// The offset of weight i stays constant
	float	*p   = &P[startWeight],
			*lim = &P[stopWeight];

	for(; p < lim; p++)
	{
		int n		= 0,
			*i		= rank+1,
			*iLim	= rank+pop-lambda;
		double prob = 1.0 + probStep;
		for(; i < iLim; i++, prob+=probStep)
		{
		//----
		//	Mark weights for permutation. Likelyhood is inversely proportional to fitness.
			if(rng.rand12() < prob)
				perm[n++] = *i;
		}

		if(n>1)
		{
			rng.shuffle<int>(perm,n);

			int	*j		= perm,				// pointer to the indices
					*jLim	= perm+n;
			float	*dst	= p + m * (*j++),	// first sample to be be shuffled
					 tmp	= *dst,
					*src;

			while(j < jLim)
			{
				src = p + m * (*j++);
			  *dst = *src;
				dst =  src;
			}
			  *dst = tmp;

		}//shuffle instances of weight i between selected individuals

	}//for all weights

}//CoSyNE::samplesPermute
